using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Diagnostics;
using Microsoft.Ccr.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using W3C.Soap;

namespace NestedParticleFilter
{
    [Contract(Contract.Identifier)]
    [DisplayName("NestedParticleFilter")]
    [Description("NestedParticleFilter service (no description provided)")]
    class NestedParticleFilterService : DsspServiceBase
    {
        /// <summary>
        /// Service state
        /// </summary>
        [ServiceState]
        NestedParticleFilterState _state = new NestedParticleFilterState();

        /// <summary>
        /// Main service port
        /// </summary>
        [ServicePort("/NestedParticleFilter", AllowMultipleInstances = true)]
        NestedParticleFilterOperations _mainPort = new NestedParticleFilterOperations();

        /// <summary>
        /// Service constructor
        /// </summary>
        public NestedParticleFilterService(DsspServiceCreationPort creationPort)
            : base(creationPort)
        {
        }

        /// <summary>
        /// Service start
        /// </summary>
        protected override void Start()
        {

            // 
            // Add service specific initialization here
            // 

            base.Start();
            Console.Write("Ummmm.. anyone there?");
            Point p1 = new Point(11,7.4); //near pink 3
            Point p2 = new Point(9,7.4);
            Point p3 = new Point(3,7.4);
            List<Landmark> landmarks =
                Landmark.ReadBelievedLandmarks(
                    "C:\\Users\\Anousha\\Microsoft Robotics Dev Studio 4 Beta 2\\Projects\\Environment\\landmarks.txt");
           
            Landmark l1 = Landmark.ClosestLandmarkByDistance(landmarks, new RobotPose(p1.X, p1.Z, 0));
            Debug.WriteLine("Closest landmark to p1: " +l1.ToString()
                            );
            Debug.WriteLine("Theta to rotate: "+ RobotPose.RotateTowardObject(new RobotPose(p1.X, p1.Z, 0), l1.Center ));

            Landmark l2 = Landmark.ClosestLandmarkByDistance(landmarks, new RobotPose(p2.X, p2.Z, 0));
            Debug.WriteLine("Closest landmark to p2: " +l2.ToString());
            Debug.WriteLine("Theta to rotate: " + RobotPose.RotateTowardObject(new RobotPose(p2.X, p2.Z, 0), l2.Center));

            Landmark l3 = Landmark.ClosestLandmarkByDistance(landmarks, new RobotPose(p3.X, p3.Z, 0));
            Debug.WriteLine("Closest landmark to p3: " +l3);
            Debug.WriteLine("Theta to rotate: " + RobotPose.RotateTowardObject(new RobotPose(p3.X, p3.Z, 0), l3.Center));
        }
    }
}


